package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;

public class BackwardAStarModule extends AbstractRoutingModule
{
	protected boolean isPF = false;
	public BackwardAStarModule(NAITOAgent owner, ConnectivityGraph graph){
		super(owner, graph);
		if(owner instanceof NAITOPoliceForce){
			isPF = true;
		}
	}
    
    public List<EntityID> search(EntityID start, EntityID goal){
    	return search(start, Arrays.asList(goal));
    }
    // Back-ward search.
	public List<EntityID> search(EntityID start, Collection<EntityID> goals){
        PriorityQueue<SearchNode> openList = new PriorityQueue<SearchNode>();
        Map<EntityID, SearchNode> closedList = new HashMap<EntityID, SearchNode>();
        int prunedCount = 0;
        int expandedCount = 0;
        long startTime, stopTime;
        String context = "BackwardAStar#backSearch";
        
        //logger.info("----", context);
        
        // reverse search: add all goals to the open list
        startTime = System.currentTimeMillis();
        for (EntityID id: goals)
        {
        	int heuristicValue = model.getDistance(id, start);
            // int heuristicValue = distanceMatrix.getDistance(id, start);
            openList.add(new SearchNode(id, null, 0, heuristicValue));
        }
        
        SearchNode currentNode = null;
        boolean searchComplete = false;
        Set<EntityID> neighbors = null;
        while (! openList.isEmpty() && ! searchComplete)
        {
            currentNode = openList.remove();
            if (currentNode.getNodeID().equals(start))
            {
                searchComplete = true;
                break;
            }
            
            //logger.debug("currentNode = " + currentNode, context);
            // check if this is closed
            if (closedList.containsKey(currentNode.getNodeID()))
            {
            	//logger.debug("closedList contains " + currentNode.getNodeID(), context+" if(contains)");
                // check distance of the previous path found to this node
                SearchNode previousNode = closedList.get(currentNode.getNodeID());
                
                //logger.debug("previousNode path length = " + previousNode.getPathLength(), context+" if(contains)");
                //logger.debug("currentNode path length = " + currentNode.getPathLength(), context+" if(contains)");
                if (previousNode.getPathLength() > currentNode.getPathLength())
                {
                	prunedCount++;
                    continue;
                }
            }
            
            // put current node on close list
            expandedCount++;
            closedList.put(currentNode.getNodeID(), currentNode);
            
            // expand node
            neighbors = graph.getNeighbors(currentNode.getNodeID());
            
            //logger.debug("neighbors = " + neighbors, context);
            for (EntityID id: neighbors)
            {
                if (! closedList.containsKey(id))
                {
                	if(checker.isPassableTo(currentNode.getNodeID(), id) || isPF){
		                // if this neighbor is not closed, add it to the open list
		                int distanceToCurrentNode = model.getDistance(id, currentNode.getNodeID());
		                // int distanceToCurrentNode = distanceMatrix.getDistance(id, currentNode.getNodeID());
		                int heuristicValue = model.getDistance(id, start);
		                // int heuristicValue = distanceMatrix.getDistance(id, start);
		                openList.add(new SearchNode(id, currentNode, distanceToCurrentNode, heuristicValue));
                    }
                }
            }
            stopTime = System.currentTimeMillis();
        }
        if (! searchComplete)
        {
            // no path found
            // Logger.debugColor("no path found", Logger.BG_RED);
            //logger.info("No path found to " + goals + ".",context);
            //logger.info("----", context);
            return null;
        }
        // construct the path
        List<EntityID> path = new LinkedList<EntityID>();
        while (currentNode.getParent() != null)
        {
            path.add(currentNode.getNodeID());
            currentNode = currentNode.getParent();
        }
        path.add(currentNode.getNodeID());
        path.remove(0);
//        Logger.debugColor("path found. length: "+path.size(), Logger.BG_GREEN);

		int totalCost = computePathLength(path);
		
		stopTime = System.currentTimeMillis();
		long processTime = stopTime - startTime;
		//logger.info("Find path: " + ", Total cost = " + totalCost + ", expandedCount = " + expandedCount + ", processTime = " + processTime + "(ms)" + ", pruned count = " + prunedCount, context);
		//logger.info("Path = " + path, context);
		//logger.info("----", context);
        return path;
	}	
}
